#ifndef CUSTOM_POSITION_H
#define CUSTOM_POSITION_H

/* 
    Chris Bena

    Position Class.  Used to calculate the position of the robot
	given the error in the yaw.  
*/

//#include <libplayerc++/playerc++.h>

#include <iostream>

#include "args.h"

#define LIMIT 80

using namespace PlayerCc;

class Position {
private:
  double x;           // X position
  double y;           // Y position
  double yaw;         // Yaw in radians
  double oldYaw;      // the previous yaw.  Used to determine when the robot turned.
  double yawCheck1;   // the upper limit of the yaw
  double yawCheck2;   // the lower limit of the yaw
  double distance;    // the distance based off the x and the y
  double lastKnownX;  // the previous X.  Used to calc the distance
  double lastKnownY;  // the previous Y.  Used to calc the distance
public:	
  Position(){
    x=0;
    y=0;
    yaw=0;
    oldYaw=0;
    yawCheck1=0;
    yawCheck2=0;
    distance=0;
    lastKnownX=0;
    lastKnownY=0;
  }

  void update(double yw, double x0, double y0){  
    // update the new position (x,y) by using the total distance traveled
    //   based off of the internal yaw
    int debug=1;  // debug value 0=no 1=yes
    yawCheck1=oldYaw+dtor(LIMIT);  // calc the upper limit of the yaw
    if(yawCheck1>dtor(360))  // check for degree wrapping
      yawCheck1=yawCheck1-dtor(360); 
    yawCheck2=oldYaw-dtor(LIMIT);  // calc lower limit of the yaw
    if(yawCheck2<0)  // check for degree wrapping
      yawCheck2=yawCheck2+dtor(360);
    if ((fabs(yw)>yawCheck1) && (fabs(yw-yawCheck1)<dtor(15))){  
	/* If the yaw exceeds the upper limit and is within 15 degrees,
		the robot has turned left.  +90 degreed
	*/
      std::cout << "**************************************************" << std::endl;
      std::cout << "yw: " << rtod(yw)<< " > oldYaw: " << rtod(oldYaw) << std::endl;
      std::cout << "**************************************************" << std::endl;
      if(yaw<dtor(270))
	yaw=yaw+dtor(90);
      else if(yaw==dtor(270))
	yaw=0; //-yaw+(PI/2);
      oldYaw=yw;
      debug=1;
    }
    else if ((fabs(yw)<yawCheck2) && (fabs(yw-yawCheck2)<dtor(15))){
	/* If the yaw has exceeded the lower limit and is within 15 degrees,
		the robot has turned right.  -90 degrees
	*/
      std::cout << "****************************************************" << std::endl;
      std::cout << "yw: " << rtod(yw)<< " < oldYaw: " << rtod(oldYaw) << std::endl;
      std::cout << "****************************************************" << std::endl;
       if(yaw>0)
       	yaw=yaw-dtor(90);
       else if(yaw==0)
	 yaw=dtor(270);
       oldYaw=yw;
      debug=1;
    }
    if (debug==1)  // debug prompt
       std::cout << "yw= " << rtod(yw) 
		<< " oldYaw=" << rtod(oldYaw) 
		<< " lowerBound= " << rtod(yawCheck2)
		<< " upperBound= " << rtod(yawCheck1)
		<< "  yaw= " << rtod(yaw) 
		<< std::endl;
    distance=sqrt((x0-lastKnownX)*(x0-lastKnownX)+(y0-lastKnownY)*(y0-lastKnownY));
    y=y+distance*sin(yaw);
    x=x+distance*cos(yaw);
    lastKnownX=x0;
    lastKnownY=y0;
  }

  void reset(){
    // reset the distance after a change in direction
    distance=0;
  }
  double getYaw(){
    // get the private value of yaw
    return yaw;
  }
  double getX(){
    // get the private value of x
    return x;
  }
  double getY(){
    // get the private value of the y
    return y;
  }
};

#endif
